#include  "include/common.h"
#include  "include/topo_exploration.h"
#include "include/topo_exploration_interface.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <iostream>

#define MIN(a, b)       ((a < b)? (a) : (b))

//根据agv的热力信息，探索脱困路径
int GeneratePath(
    G_TopoPoint_PTR topo_points, 
    int* topoPointsSize,
    G_Point2d_PTR xdcCurPoint, 
    G_AGVInfo_PTR agvInfos, 
    int* agvSize, 
    G_TopoPoint_PTR unauthorized_topo_point, 
    int* pointSize,
    G_PATH_ARR_PTR topo_exploration_path, 
    G_MAP_INFO_PTR map_info){

    std::cout << "GeneratePath start." << std::endl;

    std::vector<TopoPoint> topoPoints;

    void* ptr = new TopoExploration();
    TopoExploration *topoExploration = static_cast<TopoExploration*> (ptr);

    std::cout << "construct topo map start." << std::endl;
    std::cout << "topoPointsSize:"<< *topoPointsSize << std::endl;
    for (int i = 0; i < *topoPointsSize; i++)
    {
        G_TopoPoint topo_point = topo_points[i];
        std::vector<Neighbor> neighbors;
        neighbors.clear();
        std::string name (topo_point.name);
        for(int j = 0; j < topo_point.pt_size; j++){
            G_Neighbor g_neighbor = topo_point.neighbors[j];
            std::vector<Point2d> controlPoints;
            controlPoints.clear();
            for (int k = 0; k < g_neighbor.pt_size; k++)
            {
                controlPoints.push_back(Point2d(g_neighbor.control_points[k].x, g_neighbor.control_points[k].y));
            }
            
            neighbors.push_back(Neighbor(g_neighbor.name, controlPoints));
        }
        if (topo_point.type == 1)
        {
            topoPoints.push_back(TopoPoint(name,  TopoPointType::WAY_POINT, Point2d(topo_point.point.x, topo_point.point.y), neighbors));
        }else if (topo_point.type == 2)
        {
            topoPoints.push_back(TopoPoint(name,  TopoPointType::TASK_POINT, Point2d(topo_point.point.x, topo_point.point.y), neighbors));
        }else{
            topoPoints.push_back(TopoPoint(name,  TopoPointType::CONTROL_POINT, Point2d(topo_point.point.x, topo_point.point.y), neighbors));
        }
    }
    
    std::cout << "construct agvinfo start." << std::endl;
    std::vector<AGVInfo> agvVector;
    for (int l = 0; l < *agvSize; l++)
    {
        G_AGVInfo agvInfo = agvInfos[l];
        std::vector<AGVPathPoint> agvPathPointVector;
        for(int m = 0; m < agvInfo.pt_size; m++){
            G_AGVPathPoint agvPathPoint = agvInfo.path_points[m];
            agvPathPointVector.push_back(AGVPathPoint(Point2d(agvPathPoint.point.x, agvPathPoint.point.y),  agvPathPoint.is_topo_point, agvPathPoint.name));
        }
        
        agvVector.push_back(AGVInfo(Point2d(agvInfo.cur_pos.x, agvInfo.cur_pos.y),  agvInfo.cur_speed, agvInfo.wait_time, agvPathPointVector));
    }
    std::cout << "construct faild topo start." << std::endl;
    std::vector<Neighbor> neighbor;
    for (int n = 0; n < unauthorized_topo_point[0].pt_size; n++)
    {
        G_Neighbor neighbour = unauthorized_topo_point[0].neighbors[n];
        std::vector<Point2d> controlPoints;
        for (int p = 0; p < neighbour.pt_size; p++)
        {
            controlPoints.push_back(Point2d(neighbour.control_points[p].x,neighbour.control_points[p].y));
        }
        neighbor.push_back(Neighbor(neighbour.name, controlPoints));
    }

    TopoPoint requestFailed(unauthorized_topo_point[0].name, TopoPointType::WAY_POINT, Point2d(unauthorized_topo_point[0].point.x, unauthorized_topo_point[0].point.y),  neighbor);
    std::vector<PathWithAttribute> pathFinds;
    MapInfo mapInfo;
    mapInfo.map_path = map_info->map_path;
    mapInfo.left_boundary = map_info->left_boundary;
    mapInfo.down_boundary = map_info->down_boundary;
    mapInfo.resolution = map_info->resolution;
    std::cout << " map_path:" << mapInfo.map_path<<std::endl;
    std::cout << " call start." << std::endl;
    bool result = topoExploration->GenerateTopoExplorationPaths(topoPoints,
     Point2d(xdcCurPoint->x, xdcCurPoint->y),  
     agvVector,  
    requestFailed,  
    mapInfo, 
    10.0, 
    pathFinds);
    std::cout << " call end."<< result << std::endl;
    if (result)
    {
        int q = 0;
        for (; q < pathFinds.size(); q++)
        {
            if(q >= PATH_MAX_SIZE){
                    break;
            }
            PathWithAttribute pathFind = pathFinds[q];

            std::cout << " pathPoint size:"<< pathFind.path.size()<< std::endl;
            std::cout << " pathPoint cost:"<< pathFind.cost<< std::endl;
            int z = 0;
            for (; z < pathFind.path.size(); z++)
            {
                
                if(z >= PATH_POINT_SIZE)
                {
                    break;
                }
                memset(topo_exploration_path->paths[q].path_points[z].name, 0x00, POINT_NAME_SIZE);
                strncpy(topo_exploration_path->paths[q].path_points[z].name, 
                pathFind.path[z].GetName().c_str(), 
                MIN(POINT_NAME_SIZE-1, pathFind.path[z].GetName().size() + 1));
            }
            topo_exploration_path->paths[q].pt_size = z;
            topo_exploration_path->paths[q].cost = pathFind.cost;
        }
        topo_exploration_path->pt_size = q;
        return 1;
    }
    return 0;
    
}
     